//
// Created by augtons on 2022/1/18.
//

#include "imu.h"

static float invSqrt(float x){
    float half_x = 0.5f * x;
    float y = x;
    long i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (1.5f - (half_x * y * y));
    return y;
}

/**
 * @brief 更新四元数，结果一定返回“规范四元数”
 * @param[in] imu   IMU对象
 * @param[in] accel 加速度, 单位为g即可，因为结果一定返回“规范四元数”
 * @param[in] gyro 角速度，单位为弧度每秒和度每秒均可，因为结果一定返回“规范四元数”
 */
void IMU_Update(Imu *imu, Vector3 *accel, Vector3 *gyro){
    Quaternion preQuaternion = imu->quaternion;
    if(imu->lastTime == 0){
        imu->quaternion.q0 = 1;
        imu->quaternion.q1 = 0;
        imu->quaternion.q1 = 0;
        imu->quaternion.q1 = 0;
        imu->lastTime = esp_timer_get_time();
        return;
    }
    float halfT = (float)(esp_timer_get_time() - imu->lastTime) / 2000000;
#define q(n)    (preQuaternion.q##n)
#define a(d)    (accel->d)
    const float accelNorm = invSqrt(accel->x*accel->x + accel->y*accel->y + accel->z*accel->z);
    accel->x *= accelNorm;
    accel->y *= accelNorm;
    accel->z *= accelNorm;
#define v(d)    (gravity_accel_q.d)
    Vector3 gravity_accel_q;    // 根据四元数换算的重力加速度
    v(x) = 2*(q(1)*q(3) - q(0)*q(2));
    v(y) = 2*(q(0)*q(1) + q(2)*q(3));
    v(z) = 1 - 2*(q(1)*q(1)) - 2*(q(2)*q(2));
#define e(d)    (error.d)
    Vector3 error;  //误差
    e(x) = a(y)*v(z) - a(z)*v(y);
    e(y) = a(z)*v(x) - a(x)*v(z);
    e(z) = a(x)*v(y) - a(y)*v(x);
#define eInt(d) (imu->error_Int.d)
#define Ki  (imu->ki)
#define Kp  (imu->kp)
    eInt(x) += e(x) * Ki*halfT;
    eInt(y) += e(y) * Ki*halfT;
    eInt(z) += e(z) * Ki*halfT;
#define g(d)    (gyro->d)
    g(x) = g(x) + Kp*e(x) + eInt(x);
    g(y) = g(y) + Kp*e(y) + eInt(y);
    g(z) = g(z) + Kp*e(z) + eInt(z);
#define qn(n)    (imu->quaternion.q##n)

    qn(0) = q(0) + (-q(1)*g(x) - q(2)*g(y) - q(3)*g(z))*halfT;
    qn(1) = q(1) + (q(0)*g(x) + q(2)*g(z) - q(3)*g(y))*halfT;
    qn(2) = q(2) + (q(0)*g(y) - q(1)*g(z) + q(3)*g(x))*halfT;
    qn(3) = q(3) + (q(0)*g(z) + q(1)*g(y) - q(2)*g(x))*halfT;

    const float qNorm = invSqrt(qn(0)*qn(0) + qn(1)*qn(1) + qn(2)*qn(2) + qn(3)*qn(3));
    qn(0) *= qNorm;
    qn(1) *= qNorm;
    qn(2) *= qNorm;
    qn(3) *= qNorm;
#undef q
#undef qn
#undef a
#undef v
#undef e
#undef Ki
#undef Kp
#undef eInt
#undef g
    imu->lastTime = esp_timer_get_time();
}


/**
 * @brief 根据规范四元数求欧拉角，单位为度, Pitch and Roll
 * @param[in] quaternion
 * @param[out] eulerAngle
 */
void IMU_GetEulerAngle(Quaternion *quaternion, EulerAngle *eulerAngle){
#define q(n)    (quaternion->q##n)
    eulerAngle->roll = atan2f(
        (q(0)*q(1) + q(2)*q(3))*2,          //2(q0q1 + q2q3)
        1.0f - (q(1)*q(1) + q(2)*q(2))*2    //1 - 2(q1^2 + q2^2)
    ) * 57.3f;
    eulerAngle->pitch = asinf((q(0)*q(2) - q(1)*q(3)) * 2) * 57.3f;
#undef q
}

void IMU_GetTiltAngle(Quaternion *quaternion, float *tiltAngle, float *azimuth){
//    float q02 = quaternion->q0 * quaternion->q0;
    float q12 = quaternion->q1 * quaternion->q1;
    float q22 = quaternion->q2 * quaternion->q2;
//    float q32 = quaternion->q3 * quaternion->q3;

    float q0q1 = quaternion->q0 * quaternion->q1;
    float q0q2 = quaternion->q0 * quaternion->q2;
//    float q0q3 = quaternion->q0 * quaternion->q3;

//    float q1q2 = quaternion->q1 * quaternion->q2;
    float q1q3 = quaternion->q1 * quaternion->q3;
    float q2q3 = quaternion->q2 * quaternion->q3;

//    //float m = 4*(q02*q12+q02*q22+q02*q32 + q12*q12 + 2*q12*q22 + 2*q12*q32 - q12 + q22*q22 + 2*q22*q32 - q22 + q32*q32 - q32) + 1;
//    float z1 = 2*(2*q02*q1q3 + q0q2 + 2*q12*q1q3 + 2*q22*q1q3 + 2*q1q3*q32 - q1q3);
//    float z2 = 2*(2*q02*q2q3 - q0q1 + 2*q12*q2q3 + 2*q22*q2q3 + 2*q2q3*q32 - q2q3);
//    float z3 = 2*(2*q02*q32 + 2*q12*q32 - q12 + 2*q22*q32 - q22 + 2*q32*q32 - 2*q32) + 1;
//
//    *tiltAngle = 57.3f * acosf(z3 * invSqrt(z1*z1 + z2*z2 + z3*z3));
//    //*azimuth = 57.3f * atan2f(z2, z1);

    float m = -2*q12 - 2*q22 + 1;
    float z1 = q0q1 + q2q3;
    float z2 = -q0q2 + q1q3;

    *tiltAngle = 57.3f * acosf(m * invSqrt(4*(z1*z1 + z2*z2) + m*m));
    *azimuth = 57.3f * atan2f(z1, z2);
}
